#include "feMatrix.h"
#include "feVector.h"
#include <emmintrin.h>

//-------------------------------------------------------------------------------------------------------------------------------------
//class feDenseMatrix
//-------------------------------------------------------------------------------------------------------------------------------------
feDenseMatrix::feDenseMatrix(int iN)
{
	N = 0;
	Allocate(iN);
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Zero()
{
	for(int i=0; i<N; i++)
	{
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Fill(feReal **iFrom)
{
	for(int i=0; i<N; i++)
		CopyMemory(&A[i][0], &iFrom[i][0], N*sizeof(feReal));
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feDenseMatrix::LuDecomposition()
{
	//perform simple LU decomposition
	for(int i=0; i<N; i++)
	{
		feReal sum_diag = 0;

		for(int j=0; j<i; j++)
		{
			feReal sum_down = 0;

			//down triangle
			for(int k=0; k<j; k++)
				sum_down += A[i][k]*A[k][j];

			A[i][j] -= sum_down;


			//up triangle
			feReal sum_up = 0;
			for(int k=0; k<j; k++)
				sum_up += A[j][k]*A[k][i];
			
			A[j][i] -= sum_up;
			A[j][i] /= A[j][j];


			//to diag sum
			sum_diag += A[i][j] * A[j][i];
		}
		
		//diag element
		A[i][i] -= sum_diag;
	}

	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feDenseMatrix::ForwardTrace(feVector *iF, feVector *oX)
{
	for(int i=0; i<N; i++)
	{
		double sum = 0;
		for(int j=0; j<i; j++)
			sum += A[i][j] * oX->Data[j];
		oX->Data[i] = (iF->Data[i] - sum) / A[i][i];
	}
	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feDenseMatrix::BackwardTrace(feVector *iF, feVector *oX)
{
	for(int i=N-1; i>=0; i--)
	{
		double sum = 0;
		for(int j=N-1; j>i; j--)
			sum += A[i][j] * oX->Data[j];
		oX->Data[i] = (iF->Data[i] - sum);
	}
	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::MultMv(feVector *iV, feVector *oV)
{
	for(int i=0; i<N; i++)
	{
		oV->Data[i] = 0;
		for(int j=0; j<N; j++)
			oV->Data[i] += A[i][j] * iV->Data[j];
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::MultMAlpha(feReal iAlpha) 
{
	for(int i=0; i<N; i++)
		for(int j=0; j<N; j++)
			A[i][j] *= iAlpha;
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Transpose(feDenseMatrix *oMatrix)
{
	for(int i=0; i<N; i++)
	{
		for(int j=0; j<N; j++)
		{
			oMatrix->A[i][j] = A[j][i];
		}
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Square(feDenseMatrix *oM)
{
	for(int i=0; i<N; i++)
	{
		for(int j=0; j<N; j++)
		{
			oM->A[i][j] = 0;
			for(int k=0; k<N; k++)
				oM->A[i][j] += A[i][k]*A[i][k];
		}
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Allocate(int iSize)
{
	if(N != 0) Free();

	N = iSize;
	A = new feReal* [N];
	for(int i=0; i<N; i++)
	{
		A[i] = new feReal[N];
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feDenseMatrix::Free()
{
	for(int i=0; i<N; i++)
		delete [] A[i];
	delete [] A;
	A = 0;
	N = 0;
}


//--------------------------------------------------------------------------------------------------------------------------------------
feDenseMatrix::~feDenseMatrix()
{
	Free();
}



//-------------------------------------------------------------------------------------------------------------------------------------
//class feFiveDiagSymMatrix
//-------------------------------------------------------------------------------------------------------------------------------------
feFiveDiagSymMatrix::feFiveDiagSymMatrix()
{
	N = 0;
}


//--------------------------------------------------------------------------------------------------------------------------------------
feFiveDiagSymMatrix::feFiveDiagSymMatrix(int iN)
{
	Allocate(iN);
}


//--------------------------------------------------------------------------------------------------------------------------------------
feFiveDiagSymMatrix::feFiveDiagSymMatrix(feFiveDiagSymMatrix& iM)
{
	N = iM.N;;
	for(int i=0; i<3; i++)
	{
		A[i] = new double[N];
		CopyMemory(&A[i][0], &iM.A[i][0],  N*sizeof(double));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::Allocate(int iN)
{
	N = iN;
	for(int i=0; i<3; i++)
	{
		A[i] = new double[N];
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::Zero()
{
	for(int i=0; i<3; i++)
	{
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
feReal* feFiveDiagSymMatrix::_IJ(int iI, int iJ)	
{
	if(iI < iJ) return NULL;

	//get diag num
	int dn = iI-iJ;

	//check borders
	if(	dn <= 2)
	{
		return &A[dn][iI-dn];
	}

	return NULL; 
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::AddM(feMatrix *iMtoAdd, double iAlpha)
{
	feFiveDiagSymMatrix *m = dynamic_cast<feFiveDiagSymMatrix *>(iMtoAdd);
	if(!m)
	{
		feMatrix::AddM(iMtoAdd, iAlpha);
		return;
	}

	//add matrix
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<N; j++)
			A[i][j] += m->A[i][j]*iAlpha;
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::MultMv(feVector *iV, feVector *oV)
{
	ZeroMemory(&oV->Data[0], sizeof(oV->Data[0])*oV->GetSize());

	for(int j=0; j<N-2; j++)
	{
		oV->Data[j] += A[0][j]*iV->Data[j] + A[1][j]*iV->Data[j+1] + A[2][j]*iV->Data[j+2];
		oV->Data[j+1] += A[1][j]*iV->Data[j];
		oV->Data[j+2] += A[2][j]*iV->Data[j];
	}


	//last elements
	oV->Data[N-2] += A[0][N-2]*iV->Data[N-2] + A[1][N-2]*iV->Data[N-1];
	oV->Data[N-1] += A[1][N-2]*iV->Data[N-2];
	oV->Data[N-1] += A[0][N-1]*iV->Data[N-1];
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::MultMAlpha(feReal iAlpha)
{
	//add matrix
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<N; j++)
			A[i][j] *= iAlpha;
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::AddLocalToGlobal(feDenseMatrix *iM, int *iMRemap, double iMultCoeff)
{
	//add using remap
	for(int i=0; i<iM->GetSize(); i++)
	{
		for(int j=0; j<=i; j++)
		{
			feReal *elem = _IJ(iMRemap[i], iMRemap[j]);
			if(elem) (*elem) += *(iM->_IJ(i, j)) * iMultCoeff;
		}		
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feFiveDiagSymMatrix::LuDecomposition()			
{
	//decomposition
	for(int i=0; i<N-2; i++)
	{
		A[1][i] /= A[0][i]; 
		A[2][i] /= A[0][i]; 

		A[0][i+1] -= A[1][i] * A[1][i] * A[0][i];
		A[1][i+1] -= A[1][i] * A[2][i] * A[0][i];	

		A[0][i+2] -= A[2][i] * A[2][i] * A[0][i];
	}


	//last iters
	A[1][N-2] /= A[0][N-2]; 
	A[0][N-1] -= A[1][N-2] * A[1][N-2] * A[0][N-2];


	//done
	return true; 
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feFiveDiagSymMatrix::ForwardTrace(feVector *iF, feVector *oX)
{
	(*oX)[0] = (*iF)[0];
	(*oX)[1] = (*iF)[1] - A[1][0] * (*oX)[0];
	for(int i=2; i<N; i++)
		(*oX)[i] = (*iF)[i] - A[2][i-2] * (*oX)[i-2] - A[1][i-1] * (*oX)[i-1];

	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feFiveDiagSymMatrix::BackwardTrace(feVector *iF, feVector *oX)
{
	(*oX)[N-1] = (*iF)[N-1] / A[0][N-1];
	(*oX)[N-2] = (*iF)[N-2] / A[0][N-2] - A[1][N-2] * (*oX)[N-1];
	for(int i=N-3; i>=0 ;i--)
		(*oX)[i] = (*iF)[i] / A[0][i] - A[2][i] * (*oX)[i+2] - A[1][i] * (*oX)[i+1];

	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feFiveDiagSymMatrix::Free()
{	
	if(A)
	{
		for(int i=0; i<3; i++)
			delete [] A[i];
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
feFiveDiagSymMatrix::~feFiveDiagSymMatrix()
{
	Free();
}



//-------------------------------------------------------------------------------------------------------------------------------------
//class feThreeDiagSymMatrix
//-------------------------------------------------------------------------------------------------------------------------------------
feThreeDiagSymMatrix::feThreeDiagSymMatrix()
{
	ZeroMemory(A, 2*sizeof(A[0]));
	N = 0;
}


//--------------------------------------------------------------------------------------------------------------------------------------
feThreeDiagSymMatrix::feThreeDiagSymMatrix(int iN)
{
	Allocate(iN);
}


//--------------------------------------------------------------------------------------------------------------------------------------
feThreeDiagSymMatrix::feThreeDiagSymMatrix(feThreeDiagSymMatrix& iM)
{
	N = iM.N;;
	for(int i=0; i<2; i++)
	{
		A[i] = new double[N];
		CopyMemory(&A[i][0], &iM.A[i][0],  N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::Allocate(int iN)
{
	Free();

	N = iN;
	for(int i=0; i<2; i++)
	{
		A[i] = new double[N];
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::Zero()
{
	for(int i=0; i<2; i++)
	{
		ZeroMemory(&A[i][0], N*sizeof(feReal));
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
feReal* feThreeDiagSymMatrix::_IJ(int iI, int iJ)	
{
	if(iI < iJ) return NULL;

	//get diag num
	int dn = iI-iJ;

	//check borders
	if(	dn <= 1)
	{
		return &A[dn][iI-dn];
	}

	return NULL; 
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::AddM(feMatrix *iMtoAdd, double iAlpha)
{
	feThreeDiagSymMatrix *m = dynamic_cast<feThreeDiagSymMatrix *>(iMtoAdd);
	if(!m)
	{
		feMatrix::AddM(iMtoAdd, iAlpha);
		return;
	}

	//add matrix
	for(int i=0; i<2; i++)
	{
		for(int j=0; j<N; j++)
			A[i][j] += m->A[i][j]*iAlpha;
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feThreeDiagSymMatrix::LuDecomposition()			
{
	//decomposition
	for(int i=0; i<N-1; i++)
	{
		A[1][i] /= A[0][i]; 	
		A[0][i+1] -= A[1][i] * A[1][i] * A[0][i];	
	}


	//done
	return true; 
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feThreeDiagSymMatrix::ForwardTrace(feVector *iF, feVector *oX)
{
	oX->Data[0] = iF->Data[0];
	for(int i=1; i<N; i++)
		oX->Data[i] = iF->Data[i] - A[1][i-1] * oX->Data[i-1] ;

	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
bool feThreeDiagSymMatrix::BackwardTrace(feVector *iF, feVector *oX)
{
	oX->Data[N-1] = iF->Data[N-1] / A[0][N-1];
	for(int i=N-2; i>=0 ;i--)
		oX->Data[i] = iF->Data[i] / A[0][i] - A[1][i] * oX->Data[i+1];

	return true;
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::Free()
{	
	for(int i=0; i<2; i++)
	{
		if(A[i])
		{
			delete [] A[i];
			A[i] = 0;
		}
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::MultMv(feVector *iV, feVector *oV)
{
	ZeroMemory(&oV->Data[0], oV->GetSize()*sizeof(feReal));


	//mult all but last element
	for(int i=0; i<N-1; i++)
	{
		oV->Data[i] += A[0][i] * iV->Data[i]  +  A[1][i] * iV->Data[i+1];
		oV->Data[i+1] += A[1][i] * iV->Data[i];
	}


	//last element
	oV->Data[N-1] += A[0][N-1] * iV->Data[N-1];
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::MultMAlpha(feReal iAlpha)
{
	//mult all but last element
	for(int i=0; i<N-1; i++)
	{
		A[0][i] *= iAlpha;
		A[1][i] *= iAlpha;
	}


	//last element
	A[0][N-1] *= iAlpha;
}


//--------------------------------------------------------------------------------------------------------------------------------------
void feThreeDiagSymMatrix::AddLocalToGlobal(feDenseMatrix *iM, int *iMRemap, double iMultCoeff)
{
	//add using remap
	for(int i=0; i<iM->GetSize(); i++)
	{
		for(int j=0; j<=i; j++)
		{
			feReal *elem = _IJ(iMRemap[i], iMRemap[j]);
			if(elem) (*elem) += *(iM->_IJ(i, j)) * iMultCoeff;
		}		
	}
}


//--------------------------------------------------------------------------------------------------------------------------------------
feThreeDiagSymMatrix::~feThreeDiagSymMatrix()
{
	Free();
}
